/*
 * File:   PrimaryStateMiddleCircle.c
 * Author: craigjb
 *
 * Created on April 18, 2013, 3:57 PM
 */

#include "board.h"
#include "states.h"
#include "sensors.h"
#include "pwm.h"
#include "tick.h"

/* ************************************************************************** */
#define MIDDLE_CIRCLE_START 1
#define MIDDLE_CIRCLE_FORWARD 2
#define MIDDLE_CIRCLE_STOP 3

/* ************************************************************************** */
static const unsigned kRightWheelOffset = 100;
static const unsigned kMiddleForwardDelay = 2000;
unsigned int currentMiddleCircleState = MIDDLE_CIRCLE_START;
unsigned long int lastMiddleCircleUpdate = 0;

/* ************************************************************************** */
void MiddleCircleStart();
void MiddleCircleFoward();
void MiddleCircleStop();

/* ************************************************************************** */
void PrimaryStateMiddleCircle(void)
{
    switch (currentMiddleCircleState)
    {
        case MIDDLE_CIRCLE_START:
            MiddleCircleStart();
            break;
        case MIDDLE_CIRCLE_FORWARD:
            MiddleCircleFoward();
            break;
        case MIDDLE_CIRCLE_STOP:
            MiddleCircleStop();
            break;
        default:
            currentMiddleCircleState = MIDDLE_CIRCLE_START;
    }
}

void MiddleCircleStart()
{
    lastMiddleCircleUpdate = GetTick();
    OC1R = 2350 - kRightWheelOffset;
    OC2R = 2350;
    Forward();
    currentMiddleCircleState = MIDDLE_CIRCLE_FORWARD;
}

void MiddleCircleFoward()
{
    if (GetTick() < lastMiddleCircleUpdate + kMiddleForwardDelay)
        return;
    lastMiddleCircleUpdate = GetTick();

    currentMiddleCircleState = MIDDLE_CIRCLE_STOP;
}

void MiddleCircleStop()
{
    Stop();
}
